#include "TB6612.h"

static void (*PWM_Controller)(unsigned short, unsigned short) = (void*)0;

void TB6612_RegisterPWMController(void (*handler)(unsigned short, unsigned short))
{
    PWM_Controller = handler;
}

void TB6612_Stop(TB6612DeviceTypeDef *device , TB6612Channel_TypeDef Channelx)
{
    PORT_TypeDef * Control1, * Control2;
    Control1    = (void*)((unsigned int)device + (sizeof(PORT_TypeDef) * 2) * Channelx) ;
    Control2    = (void*)((unsigned int )Control1 + sizeof(PORT_TypeDef))               ;
    PORTManage_SetBit(Control1);
    PORTManage_SetBit(Control2);
}

void TB6612_Free(TB6612DeviceTypeDef *device , TB6612Channel_TypeDef Channelx)
{
    PORT_TypeDef * Control1, * Control2;
    Control1    = (void*)((unsigned int)device + (sizeof(PORT_TypeDef) * 2) * Channelx) ;
    Control2    = (void*)((unsigned int )Control1 + sizeof(PORT_TypeDef))               ;
    PORTManage_ClearBit(Control1);
    PORTManage_ClearBit(Control2);
}

void TB6612_GoForward(TB6612DeviceTypeDef *device , TB6612Channel_TypeDef Channelx)
{
    PORT_TypeDef * Control1, * Control2;
    Control1    = (void*)((unsigned int)device + (sizeof(PORT_TypeDef) * 2) * Channelx) ;
    Control2    = (void*)((unsigned int )Control1 + sizeof(PORT_TypeDef))               ;
    PORTManage_SetBit(Control1);
    PORTManage_ClearBit(Control2);
}

void TB6612_GoBack(TB6612DeviceTypeDef *device , TB6612Channel_TypeDef Channelx)
{
    PORT_TypeDef * Control1, * Control2;
    Control1    = (void*)((unsigned int)device + (sizeof(PORT_TypeDef) * 2) * Channelx) ;
    Control2    = (void*)((unsigned int )Control1 + sizeof(PORT_TypeDef))               ;
    PORTManage_ClearBit(Control1);
    PORTManage_SetBit(Control2);
}

void TB6612_SetSpeed(TB6612DeviceTypeDef *device , TB6612Channel_TypeDef Channelx, unsigned short speed)
{
    unsigned short PWM_Channel;
    PWM_Channel = *((unsigned short *)((unsigned int)device + sizeof(PORT_TypeDef) * 4 + Channelx * 2));
    PWM_Controller(PWM_Channel, speed);
}
